package model;

/**
 * @author wujiazhan
 * @className FirstOrderInvertedPendulumSystem
 * @description 一阶倒立摆控制系统
 * @date 2021-06-01 15:49
 */
@SuppressWarnings("all")
public class FirstOrderInvertedPendulumSystem {

    private PIDController theta_PID;
    private PIDController x_PID;
    private InvertedPendulum invertedPendulum;
    private Differential theta_diff;
    private Differential x_diff;
    private Integrator integrator;
    private double x_target = 0.0;
    private double theta = 0.0;
    private double theta_pre = 0.0;
    private double theta_1 = 0.0;
    private double x_car = 0.0;
    private double x_pre = 0.0;
    private double x_1 = 0.0;
    private boolean is_swingUP = true; //是否处于起摆状态
    private boolean is_impulse = true;//是否脉冲
    private int impulseNum = 1;

    public FirstOrderInvertedPendulumSystem(double samplingTime){
        this.invertedPendulum = new InvertedPendulum(new ModelParameter(1,1,0.6, Math.PI, samplingTime,9.8));
        this.theta_PID = new PIDController(3.392,0,0.75,samplingTime,1000,-1000);
        this.x_PID = new PIDController(1,0,1.55,samplingTime,1000,-1000);
        this.theta_diff = new Differential(samplingTime);
        this.x_diff = new Differential(samplingTime);
        this.integrator = new Integrator(samplingTime);
    }

    public void run(double x_tar){
        //初始化力F
        double F = 0.0;
        this.x_target = x_tar;
        if(is_swingUP) {
            F = swingUp(theta, theta_1, 350);
        } else {
            F = steadySwing(x_target, x_car, x_pre, theta, theta_pre, 0.3, -50);
        }

        invertedPendulum.forwardAndRKIntegral(F);//迭代状态
        //invertedPendulum.forwardAndIntegral(F);

        //更新摆角相关变量状态
        theta = invertedPendulum.getTheta();
        theta_pre = theta_diff.getPre_val();
        theta_1 = theta_diff.getDiff(theta);
        //更新位置相关变量状态
        x_car = invertedPendulum.getX_car();
        x_pre = x_diff.getPre_val();
        x_1 = x_diff.getDiff(x_car);

        if(Math.abs(theta) < (Math.PI / 6) && is_swingUP == true) {
            is_swingUP = false;
        }
    }

    /**
     * 能量起摆算法
     * @param theta 摆角
     * @param theta_1 摆角一阶微分
     * @param k 加速度增益
     * @return 输出起摆所需要加速度
     */
    public double swingUp(double theta, double theta_1, double k){

        double kinetic_energy = (1.0 / 240) * Math.pow(theta_1,2); //动能
        double potential_energy = 0.1 * 0.25 * 9.8 * (Math.cos(theta) - 1); //势能
        int sign = theta_1 * Math.cos(theta) >= 0 ? 1 : -1; //加速度符号
        double a_output = k * sign * (kinetic_energy + potential_energy); //加速度输出
        double a_output_max = 9.8 * 2;//加速度最大为9.8 * 2
        if(a_output > a_output_max) {//限定加速度的范围
            a_output = a_output_max;
        } else if (a_output < -a_output_max) {
            a_output = -a_output_max;
        }
        //起摆时给一个力的脉冲
        if(is_impulse){
            a_output = 1;
            impulseNum ++;
            if(impulseNum > 2) {
                is_impulse = false;
            }
        }
        return a_output;
    }

    /**
     * 稳摆算法
     * @param x_cur 当前拍的小车位置
     * @param theta_cur 当前拍的摆角位置
     * @param outer_gain 外环增益
     * @param inner_gain 内环增益
     * @return 稳摆算法输出的力
     */
    public double steadySwing(double x_tar, double x_cur, double x_pre, double theta_cur, double theta_pre, double outer_gain, double inner_gain){
        //初始化PID内部上一拍的值
        x_PID.setDiffPre_val(x_pre);
        theta_PID.setDiffPre_val(theta_pre);

        double x_state = x_PID.forward(x_cur);//位置PID输出

        double outerLoop = (x_tar - x_state) * outer_gain;//外环PID最终输出结果
        double innerLoop = theta_PID.forward(theta_cur);//内环PID输出

        return (outerLoop - innerLoop) * inner_gain;//双闭环PID输出结果
    }

    public double getX_car(){
        return x_car;
    }

    public double getTheta(){
        return theta;
    }
    public double getX_target(){
        return x_target;
    }

}
